/*
 *  ctload.cpp
 *  EPPI-0.0
 *
 *  Created by Juan Carlos Garcia Orden on 15/11/07.
 *
 *  Modified by Markus Lutz on 15/10/09
 */

#include "ctload.h"

CTLoad::CTLoad()
:	Component(),
	value()
{}

// ***

CTLoad::CTLoad(ifstream & in, string bodyName, int di, const vector<double> &vTime, const map<string,vector<string>,less<string> >* pset) 
:	Component(),
	value(),
	Wext(0.0)
{
	dim = di;
	
	string keyword;
	string auxs;	/** Auxiliary string */
	double auxd;	/** Auxiliary double */

	char forcefile[100]; // name of the file including the force 	// edit
	ifstream inforce; // stream for the force file 			// edit
	vector<double> force (dim+1); 					// edit
	int nTimeSteps;							// edit
	int i;
	string line;
	vector<double> Fmax; 		// local vector of the maximal force given by value
	vector<string> FmaxSt;
	stringstream stnum;
	
	map<string,vector<string>,less<string> > pointSets = *pset;
	
	/** Extract the system name where the body is */
	string sysName = getStringHead(bodyName);	
	while (in >> keyword && keyword.compare("ENDLOAD") )
	{
		if(!keyword.compare("POINT"))
		{
			while (in >> auxs && (auxs.compare("VALUE") && auxs.compare("FILE"))) // edit
			{
				/** If it does not have slash, add it with the system name */
				if(checkForSlash(auxs)==false) addSystemName(auxs,sysName);
				pointNames.push_back(auxs);	
			}		
		}
		else if(!keyword.compare("POINTSET"))
		{
			in >> auxs;	// Name of pointSet
			
			for(i=0; i<pointSets[auxs].size(); ++i)	
			{
//				cout << "anadiendo punto " << pointSets[auxs][i] << " de pointset " << auxs << "\n";
				pointNames.push_back(pointSets[auxs][i]);	
			}
			in >> auxs;
		}
		if(!auxs.compare("VALUE"))
		{
			getline(in,line);
			stringstream iss(line);
			while (iss >> auxs )
			{
				FmaxSt.push_back(auxs);
			}
			for(i=0; i<FmaxSt.size(); ++i)
			{
				stnum << FmaxSt[i];
				stnum >> auxd;
				Fmax.push_back(auxd);
				stnum.clear();
			}			
			
			// check if time history exist and avoid division by zero
//			if (vTime.size() == 1 && vTime[0] == 0) // only t = 0, no time history
			if (vTime.size() == 2) // just begining and end, no time history
			{
				force[0] = vTime.back();	// 
				for (int j=1; j <= dim; j++)
				{
					force[j] = Fmax[j-1];
				}
				value.push_back(force);	
			}
			else	// vTime contains several time steps....
			{	

				for (int i = 0; i < vTime.size(); i++)	// loop over all timesteps
				{	
					force[0] = vTime[i];		// make vector: time, Fx, Fy, Fz (number of columns depend on dimension)

					for (int j=1; j <= dim; j++)
					{	
						force[j] = Fmax[j-1]*vTime[i]/vTime.back();	// calculate force for current time step	
					}
	
				value.push_back(force); 	// add vector to matrix	
				}
			}
		}
		if(!auxs.compare("FILE")) // reading force from file
		{

			in >> forcefile;	// read name of the file

//			cout << "Reading force from file...." << endl;
//			cout << forcefile << " ......." << endl;

			inforce.open(forcefile);				
			inforce >> nTimeSteps;	// read number of timesteps from file
			

			for(int k=0; k<nTimeSteps;k++) // loop over all timesteps of the force history
			{
			
				for(int i = 0; i< dim+1 ; i++)	// create matrix: time, x, y, z (number of columns depend on dimension)
				{
				inforce >> auxd;	// read from 
				force[i] = auxd;	// make vector: time, x, y, z (number of columns depend on dimension)
				}
				
				value.push_back(force); // add vector to matrix
			}
				
			inforce.close();		// close file that contains the force history
//			cout << "successful\n";
		}
		
		// and ends here.....

		// only for tests..delete later
		/*
		for (int i = 0; i<value.size();i++){
			for (int j = 0; j<=3;j++){
				cout  <<value[i][j] << "\t";
			}
			cout << "\n";
		} 
		*/
		// test ends here
	}

//	numDofs = dim*pointNames.size();
//	Ft.resize(numDofs);
}

// ***

void CTLoad::calcFi(const vector<double>& qddot)
{}

// ***

void CTLoad::calcFq(const int& nt)
{}

// ***

void CTLoad::calcFt(const double& t, const double& grav, const int& gravDir)
{
	int i=0;
	int j=0;
		
	for(i=0; i<pointNames.size(); i++)
	{
		for(j=0; j<numDofsOnePoint; j++) 
		{
			Ft[j + numDofsOnePoint*i] = getForce(t,j);
//			cout << "force at t= " << t << " is " << getForce(t, j) << endl;
		}
	}
}

// ***

void CTLoad::calcKi(double & partial_qddot)
{}

// ***

void CTLoad::calcKq(int nt)
{}

// ***

void CTLoad::calcKdq(double & partial_qdot)
{}

// ***

void CTLoad::calcM(const vector<double>& q, const double& t)
{}

// ***

void CTLoad::calcMLumped(const vector<double>& q, const double& t)
{}

// ***

void CTLoad::setDofs(void)
{
	// It must not add any dof, but must set variable numDofs
	
 	map<string,Point*,less<string> >::iterator k;
	
	numDofsOnePoint = mPoints.begin()->second->countDofs();	// It is assumed that the two points have the same number of dofs
	numDofs = 0;
	for(k=mPoints.begin();k!=mPoints.end(); k++)	
	{
		numDofs += k->second->countDofs();
	}
	//	cout << "numDofs en cload is " << numDofs << endl;	
}

// ***

void CTLoad::initializeGlobalDofs(vector<double> &q_in, vector<double> &dq_in, const double& t )
{
	int i=0;
	
	if(q.size()==0)		/** If q has not been initialized yet, put dofs to 0 */
	{
		q.resize(numDofs);
		dq.resize(numDofs);
	}
	else if(dq.size()==0)
	{
		for(i=0; i<q.size(); i++)	dq_in[ localGlob[i] ] = 0.0 ;	/** From rest. Other initial condition  TO BE DONE. */
	}		
}

// ***

void CTLoad::setMatAtComp(Material* ma)
{}

// ***

double CTLoad::getForce(const double& t, int coord) // returns linearly interpolated value of the function saved in value
{	
	if (t <= value.back()[0] && t >= value.front()[0]) // if t is smaller or bigger than the values of time given in "value", 0 is returned
	{
		int i = 0;
		while ((i < value.size()-1) && (t >= value[i][0]) ) { // finding the next value of time bigger than current t 
			i=i+1;
		} 
		//  interpolating and returning the value
		// cout << "test" << "\n"; // borrar
		if (i == 0) // no interpolations possible, as value only returns one value
		{
			return value[i][coord+1];
		}
		else
		{
			//cout << t << "\t" << value [i-1][coord+1] << "\t" << value[i][0] << "\n";
			return value[i-1][coord+1]+ (t-value[i-1][0]) * (value[i][coord+1] - value[i-1][coord+1]) / (value[i][0]-value[i-1][0]);
		// 		f(x_1)	  +    (x - x_1)   	* ( f(x_2) 	 -  f(x_1) )		/	(x_2-x_1)
		}
	}

	else 	{
		return 0;
	}	 
}

// ***

void CTLoad::initializeCompRes(const bool& st, const double& grav, const int& gravDir, const bool augLagFlag)
{
	Ft.resize(numDofs);

	if (numDofsOnePoint!=value[0].size()-1)
	{
		cout << "numDofs = " << numDofs << endl;
		cout << "value.size " << value[0].size()-1 << endl;
		cout << "ERROR** Number of dofs does not match the dimension of the load vector \n";
	}
	
}

// ***

//void CTLoad::initializeCompTang(const bool& st,  const bool& pf)
void CTLoad::initializeCompTang(const bool& st)
{}

// ***

void CTLoad::calcEner(double& Ekin, double& Epot, double& Econ, double& Eext, double& Eamb, double& Dint, double& grav, int& gravDir, vector<double>& q_in, vector<double>& dq_in)
{
        int i=0;
        int j=0;
        

        if(deltaT != 0.0)
        {
                for(i=0; i<pointNames.size(); i++)
                {
                        for(j=0; j<numDofsOnePoint; j++)
                        {
                                Wext += deltaT / 2.0 * (dq_in[localGlob[j+ numDofsOnePoint*i]] * getForce(time, j) + dqC[j+ numDofsOnePoint*i] * getForce(time - deltaT,j));
//                              Wext += deltaT * (dq_in[localGlob[j+ numDofsOnePoint*i]] * getForce(time, j));
                        }
                }
                Eext += Wext;
        }
}

// ***

void CTLoad::calcEntropy(double& Entropy, double& EntropyAmb)
{}

// ***

void CTLoad::calcAM(vector<double>& am, vector<double>& q_in, vector<double>& dq_in)
{}	

// ***

void CTLoad::updateComp(void)
{}

// ***

void CTLoad::setIniDofs(vector<double>&)
{}

// ***

void CTLoad::calcPhi(const vector<double>& q)
{}

// ***

void CTLoad::calcDPhi(const vector<double>& q)
{}

// ***

void CTLoad::calcMSmooth(const vector<double>& q_in, const double& t)
{}

// ***

void CTLoad::calcFSmooth(const vector<double>& q_in, const vector<double>& dq_in, const double& t)
{}

int CTLoad::checkState()
{
return 0;
}
